"""
工具文件
"""

import numpy as np
import logging

logger = logging.getLogger(__name__)


def project_point_to_line_segment(point, p1, p2):
    start = np.array((p1['x'], p1['y']))
    end = np.array((p2['x'], p2['y']))
    line_vec = end - start
    point_vec = np.array(point) - start
    line_len = np.dot(line_vec, line_vec)

    # Handle case when start and end points are the same
    if line_len == 0:
        return (p1['x'], p1['y'], p1['h'], p1['v'], p1['t']), 0

    interpolation_factor = np.dot(point_vec, line_vec) / line_len
    # Clamp interpolation factor to the range [0, 1]
    interpolation_factor = max(0, min(1, interpolation_factor))

    # Compute interpolated values
    x, y = start + interpolation_factor * line_vec
    begin_h = p1['h']
    if p2['h'] > 340 and p1['h'] < 20:
        p2['h'] -= 360
    elif p1['h'] > 340 and p2['h'] < 20:
        p1['h'] -= 360
    h = begin_h + interpolation_factor * (p2['h'] - p1['h'])
    v = p1['v'] + interpolation_factor * (p2['v'] - p1['v'])
    t = p1['t'] + interpolation_factor * (p2['t'] - p1['t'])

    is_on_segment = (0 <= interpolation_factor <= 1)
    if not is_on_segment:
        # If the projected point is not on the segment, return the nearest endpoint's data
        nearest_endpoint = p1 if interpolation_factor < 0 else p2
        return (nearest_endpoint['x'], nearest_endpoint['y'], nearest_endpoint['h'], nearest_endpoint['v'],
                nearest_endpoint['t']), interpolation_factor

    return (x, y, h, v, t), interpolation_factor


def is_ray_intersects_segment(point, start, end) -> bool:
    if start[1] == end[1]:  # 排除与射线平行、重合，线段首尾端点重合的情况
        return False
    if start[1] > point[1] and end[1] > point[1]:  # 线段在射线上边
        return False
    if start[1] < point[1] and end[1] < point[1]:  # 线段在射线下边
        return False
    if start[1] == point[1] and end[1] > point[1]:  # 交点为下端点，对应start point
        return False
    if end[1] == point[1] and start[1] > point[1]:  # 交点为下端点，对应end point
        return False
    if start[0] < point[0] and end[1] < point[1]:  # 线段在射线左边
        return False
    seg = end[0] - (end[0] - start[0]) * (end[1] - point[1]) / (end[1] - start[1])  # 求交
    if seg < point[0]:
        return False
    return True

def normalize_angle(angle_rad):
    """
    将角度归一化到 [-π, π] 区间
    """
    normalize_angle_ = np.fmod(angle_rad + np.pi, 2 * np.pi)
    if normalize_angle_ < 0:
        normalize_angle_ += 2 * np.pi
    return normalize_angle_ - np.pi


def interpolate_trajectory_with_heading(x, y, v, h, dt=0.1):
    """
    对包含航向角的轨迹进行时间插值
    :param x: 原始 x 坐标数组 (可能是较大值，比如 GPS 坐标)
    :param y: 原始 y 坐标数组
    :param v: 原始速度数组
    :param h: 原始航向角数组 (范围在 [-pi, pi])
    :param dt: 目标时间间隔 (默认 0.1s)
    :return: t_new, x_new, y_new, v_new, h_new
    """
    # Step 1: 将 x, y 平移到局部坐标系（以第一个点为原点）
    x_offset = x[0]  # 记录 x 的全局偏移量
    y_offset = y[0]  # 记录 y 的全局偏移量

    x_local = x - x_offset  # 平移到局部坐标系
    y_local = y - y_offset

    # Step 2: 计算每两个点之间的欧氏距离 Δs
    distances = np.sqrt(np.diff(x_local)**2 + np.diff(y_local)**2)  # 每段轨迹的实际几何距离 Δs

    # Step 3: 计算每段轨迹的时间间隔 Δt = Δs / v
    avg_v = (v[:-1] + v[1:]) / 2  # 取平均速度，避免 v=0 问题
    delta_t = distances / avg_v

    # Step 4: 累积时间，生成原始时间轴 t_orig
    t_orig = np.concatenate(([0], np.cumsum(delta_t)))

    # Step 5: 生成新的均匀时间轴 t_new
    t_new = np.arange(0, t_orig[-1], dt)

    # Step 6: 分解航向角 h 为 sin 和 cos 分量（避免插值跳变）
    sin_h = np.sin(h)
    cos_h = np.cos(h)

    # Step 7: 对局部坐标、速度和航向角分量进行线性插值
    x_local_new = np.interp(t_new, t_orig, x_local)  # 插值局部坐标 x
    y_local_new = np.interp(t_new, t_orig, y_local)  # 插值局部坐标 y
    v_new = np.interp(t_new, t_orig, v)             # 插值速度
    sin_h_new = np.interp(t_new, t_orig, sin_h)     # 插值 sin(h)
    cos_h_new = np.interp(t_new, t_orig, cos_h)     # 插值 cos(h)

    # Step 8: 重新计算航向角 h
    h_new = np.arctan2(sin_h_new, cos_h_new)

    # Step 9: 将插值后的局部坐标映射回全局坐标
    x_new = x_local_new + x_offset  # 恢复全局坐标 x
    y_new = y_local_new + y_offset  # 恢复全局坐标 y

    return t_new, x_new, y_new, v_new, h_new